﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Lung_Sounds_V2
{
    public class SoundFilter
    {
        public List<double[]> FilterRanges { get; set; }
        public double WindowSize { get; set; }

        public class FilterState
        {
            public double[] xv { get; set; }
            public double[] yv { get; set; }
        }

        public SoundFilter()
        {
            FilterRanges = new List<double[]>();
            WindowSize = 44100;
        }

        public short[] FilterWaveHighPass(short[] wave, ref FilterState state)
        {
            if (state == null)
            {
                state = new FilterState();
                state.xv = new double[9];
                state.yv = new double[9];
            }

            short[] output = new short[wave.Length];

            for (int i = 0; i < wave.Length; i++)
            {
                state.xv[0] = state.xv[1]; 
                state.xv[1] = state.xv[2]; 
                state.xv[2] = state.xv[3]; 
                state.xv[3] = state.xv[4]; 
                state.xv[4] = state.xv[5];

                state.xv[5] = wave[i] / 1.01392800;

                state.yv[0] = state.yv[1]; 
                state.yv[1] = state.yv[2]; 
                state.yv[2] = state.yv[3]; 
                state.yv[3] = state.yv[4];
                state.yv[4] = state.yv[5];

                state.yv[5] = (state.xv[5] - state.xv[0]) + 5 * (state.xv[1] - state.xv[4]) + 10 * (state.xv[3] - state.xv[2])
                     + (0.9727153410 * state.yv[0]) + (-4.8904855801 * state.yv[1])
                     + (9.8351614854 * state.yv[2]) + (-9.8897275773 * state.yv[3])
                     + (4.9723363310 * state.yv[4]);
                output[i] = (short)state.yv[5];

            }
            return output;
        }


        public short[] FilterWaveBandStop3850_3992(short[] wave, ref FilterState state)
        {
            if (state == null)
            {
                state = new FilterState();
                state.xv = new double[5];
                state.yv = new double[5];
            }

            short[] output = new short[wave.Length];
            double GAIN = 1.014204331e+00;

            for (int i = 0; i < wave.Length; i++)
            {
                state.xv[0] = state.xv[1];
                state.xv[1] = state.xv[2];
                state.xv[2] = state.xv[3];
                state.xv[3] = state.xv[4];

                state.xv[4] = wave[i] / GAIN;

                state.yv[0] = state.yv[1];
                state.yv[1] = state.yv[2];
                state.yv[2] = state.yv[3];
                state.yv[3] = state.yv[4];

                state.yv[4] = (state.xv[0] + state.xv[4]) - 3.3923610971 * (state.xv[1] + state.xv[3]) + 4.8770284532 * state.xv[2]
                             + (-0.9721853723 * state.yv[0]) + (3.3212604275 * state.yv[1])
                             + (-4.8085275866 * state.yv[2]) + (3.3684390579 * state.yv[3]);
                output[i] = (short)state.yv[4];
            }

            return output;
        }

        public short[] FilterWaveLowButter(short[] wave, ref FilterState state)
        {
            if (state == null)
            {
                state = new FilterState();
                state.xv = new double[9];
                state.yv = new double[9];
            }

            short[] output = new short[wave.Length];

            for (int i = 0; i < wave.Length; i++)
            {
                state.xv[0] = state.xv[1];
                state.xv[1] = state.xv[2];
                state.xv[2] = state.xv[3];
                state.xv[3] = state.xv[4];
                state.xv[4] = state.xv[5];
                state.xv[5] = state.xv[6];
                state.xv[6] = state.xv[7];
                state.xv[7] = state.xv[8];

                state.xv[8] = wave[i] / 1.780830605e+04;

                state.yv[0] = state.yv[1];
                state.yv[1] = state.yv[2];
                state.yv[2] = state.yv[3];
                state.yv[3] = state.yv[4];
                state.yv[4] = state.yv[5];
                state.yv[5] = state.yv[6];
                state.yv[6] = state.yv[7];
                state.yv[7] = state.yv[8];

                state.yv[8] = (state.xv[0] + state.xv[8]) + 8 * (state.xv[1] + state.xv[7]) + 28 * (state.xv[2] + state.xv[6])
                                + 56 * (state.xv[3] + state.xv[5]) + 70 * state.xv[4]
                                + (-0.0233866755 * state.yv[0]) + (0.2723188327 * state.yv[1])
                                + (-1.4140019128 * state.yv[2]) + (4.2869014449 * state.yv[3])
                                + (-8.3261655725 * state.yv[4]) + (10.6524418990 * state.yv[5])
                                + (-8.8182916170 * state.yv[6]) + (4.3558082864 * state.yv[7]);
                output[i] = (short)state.yv[8];
            }
            return output;
        }

        public short[] FilterWaveBandPass(short[] wave)
        {
            short[] output = new short[wave.Length];

            double[] xv = new double[21];
            double[] yv = new double[21];

            for (int i = 0; i < wave.Length; i++)
            { 
                xv[0] = xv[1]; xv[1] = xv[2]; xv[2] = xv[3]; xv[3] = xv[4]; xv[4] = xv[5]; xv[5] = xv[6]; xv[6] = xv[7]; xv[7] = xv[8]; xv[8] = xv[9]; xv[9] = xv[10]; xv[10] = xv[11]; xv[11] = xv[12]; xv[12] = xv[13]; xv[13] = xv[14]; xv[14] = xv[15]; xv[15] = xv[16]; xv[16] = xv[17]; xv[17] = xv[18]; xv[18] = xv[19]; xv[19] = xv[20]; 
                xv[20] = wave[i] / 2.266531064e+05;
                yv[0] = yv[1]; yv[1] = yv[2]; yv[2] = yv[3]; yv[3] = yv[4]; yv[4] = yv[5]; yv[5] = yv[6]; yv[6] = yv[7]; yv[7] = yv[8]; yv[8] = yv[9]; yv[9] = yv[10]; yv[10] = yv[11]; yv[11] = yv[12]; yv[12] = yv[13]; yv[13] = yv[14]; yv[14] = yv[15]; yv[15] = yv[16]; yv[16] = yv[17]; yv[17] = yv[18]; yv[18] = yv[19]; yv[19] = yv[20]; 
                yv[20] =   (xv[0] + xv[20]) - 10 * (xv[2] + xv[18]) + 45 * (xv[4] + xv[16])
                             - 120 * (xv[6] + xv[14]) + 210 * (xv[8] + xv[12]) - 252 * xv[10]
                             + ( -0.0097994259 * yv[0]) + (  0.2392608712 * yv[1])
                             + ( -2.7842597508 * yv[2]) + ( 20.5337099470 * yv[3])
                             + (-107.6393306700 * yv[4]) + (426.3410720100 * yv[5])
                             + (-1323.9290895000 * yv[6]) + (3300.6459849000 * yv[7])
                             + (-6709.5466296000 * yv[8]) + (11230.5973640000 * yv[9])
                             + (-15562.6020110000 * yv[10]) + (17884.1504250000 * yv[11])
                             + (-17012.4183000000 * yv[12]) + (13321.7084400000 * yv[13])
                             + (-8502.1128401000 * yv[14]) + (4353.6831332000 * yv[15])
                             + (-1746.4511715000 * yv[16]) + (528.7935993900 * yv[17])
                             + (-113.6567321000 * yv[18]) + ( 15.4571735730 * yv[19]);
                output[i] = (short)yv[20];
            }
            
            return output;
        }

        public double[] FilterWave(double[] wave)
        {
            AForge.Math.Complex[] fftComp = new AForge.Math.Complex[wave.Length];

            for (int i = 0; i < wave.Length; i++)
            {
                fftComp[i].Re = wave[i];
            }

            //fftComp = Hann(fftComp);

            AForge.Math.FourierTransform.FFT(fftComp, AForge.Math.FourierTransform.Direction.Forward);

            for (int i = 0; i < fftComp.Length; i++)
            {
                double freq;

                if (i > fftComp.Length / 2)
                {
                    freq = (double)(fftComp.Length - i) / WindowSize;
                }
                else
                {
                    freq = (double)i / WindowSize;
                }

                if  (FilterRanges.Exists(x => x[0] <= freq && x[1] >= freq))
                {
                    fftComp[i] = new AForge.Math.Complex(0, 0);
                }
            }

            // Reverse FFT
            AForge.Math.FourierTransform.FFT(fftComp, AForge.Math.FourierTransform.Direction.Backward);

            double[] output = new double[fftComp.Length];
            for (int i = 0; i < fftComp.Length; i++)
            {
                output[i] = fftComp[i].Re;
            }

            return output;
        }

        public AForge.Math.Complex[] Hann(AForge.Math.Complex[] iwv)
        {
            int N = iwv.Length;

            for (int n = 1; n < N; n++)
                iwv[n].Re = iwv[n].Re * (0.54f - 0.46f * (float)Math.Cos((2 * Math.PI * n) / (N - 1)));

            return iwv;
        }
    }
}
